#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import tf
import time
#from geometry_msgs.msg import PoseStamped


def get_robot_pose():
    # 初始化ROS节点
    rospy.init_node('find_robot_pose', anonymous=True)

    # 创建一个TransformListener对象
    listener = tf.TransformListener()

    # 设定频率（0.5秒）
      # 2 H


    try:
        listener.waitForTransform("odom", "base_link", rospy.Time(0), rospy.Duration(3.0))

        (trans, rot) = listener.lookupTransform("odom", "base_link", rospy.Time(0))

        # 获取从 'odom' 到 'base_link' 的变换，使用当前时间
        # (trans, rot) = listener.lookupTransform('odom', 'base_link', rospy.Time.now())

        # 解析平移和旋转数据
        x, y, z = trans  # 机器人在地图坐标系下的位置
        roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot)  # 获取旋转角度（yaw）

        # 打印机器人位姿
        rospy.loginfo("robot pose x: %f", x)
        rospy.loginfo("robot pose y: %f", y)
        # rospy.loginfo("robot pose z: %f", z)  # 输出Z坐标，如果有需要
        # rospy.loginfo("robot pose roll: %f", roll)  # 输出 roll 角
        # rospy.loginfo("robot pose pitch: %f", pitch)  # 输出 pitch 角
        rospy.loginfo("robot pose yaw: %f", yaw)  # 输出 yaw 角

    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
        rospy.logerr("Failed to get transform: %s", e)

        # 按照设定频率睡眠
        rate.sleep()


if __name__ == '__main__':
    #rate = rospy.Rate(10)
    while True:

        try:
            get_robot_pose()
            time.sleep(0.5)
        except rospy.ROSInterruptException:
            pass

